home *** CD-ROM | disk | FTP | other *** search
/ BCI NET 2 / BCI NET 2.iso / archives / programming / source / graphicgems4.lha / GemsIV / collide.c < prev    next >
Encoding:
C/C++ Source or Header  |  1995-02-06  |  28.5 KB  |  948 lines
  1. /*
  2.  * C code from the article
  3.  * "Fast Collision Detection of Moving Convex Polyhedra"
  4.  * by Rich Rabbitz, rrabbitz%pgn138fs@serling.motown.ge.com
  5.  * in "Graphics Gems IV", Academic Press, 1994
  6.  */
  7.  
  8. #include <math.h>
  9.  
  10. typedef long           Boolean;
  11.  
  12. #define FUZZ           (0.00001)
  13. #define TRUE           (1)
  14. #define FALSE           (0)
  15. #define MAX_VERTS      (1026)
  16.  
  17. #define ABS(x)           ( (x) > 0 ? (x) : -(x) )
  18. #define EQZ(x)           ( ABS((x)) < FUZZ ? TRUE : FALSE )
  19.  
  20. #define DOT3(u,v)      ( u[0]*v[0] + u[1]*v[1] + u[2]*v[2])
  21. #define VECADD3(r,u,v) { r[0]=u[0]+v[0]; r[1]=u[1]+v[1]; r[2]=u[2]+v[2]; }
  22. #define VECADDS3(r,a,u,v){r[0]=a*u[0]+v[0]; r[1]=a*u[1]+v[1]; r[2]=a*u[2]+v[2];}
  23. #define VECSMULT3(a,u) { u[0]= a * u[0]; u[1]= a * u[1]; u[2]= a * u[2]; }
  24. #define VECSUB3(r,u,v) { r[0]=u[0]-v[0]; r[1]=u[1]-v[1]; r[2]=u[2]-v[2]; }
  25. #define CPVECTOR3(u,v) { u[0]=v[0];     u[1]=v[1];     u[2]=v[2]; }
  26. #define VECNEGATE3(u)  { u[0]=(-u[0]);     u[1]=(-u[1]);     u[2]=(-u[2]); }
  27.  
  28. #define GET(u,i,j,s) (*(u+i*s+j))
  29. #define GET3(u,i,j,k,s) (*(u+i*(s*2)+(j*2)+k))
  30.  
  31. /**************************************************************************
  32.  *
  33.  * The structure polyhedron is used to store the geometry of the primitives
  34.  * used in this collision detection example.  Since the collision detection
  35.  * algorithm only needs to operate on the vertex set of a polyhedron, and
  36.  * no rendering is done in this example, the faces and edges of a
  37.  * polyhedron are not stored.  Adding faces and edges to the structure for
  38.  * rendering purposes should be straight forward and will have no effect on
  39.  * the collision detection computations.
  40.  *
  41.  **************************************************************************/
  42.  
  43. typedef struct polyhedron {
  44.    double   verts[MAX_VERTS][3]; /* 3-D vertices of polyhedron. */
  45.    int        m;             /* number of 3-D vertices.  */
  46.    double   trn[3];         /* translational position in world coords. */
  47.    double   itrn[3];         /* inverse of translational position. */
  48. } *Polyhedron;
  49.  
  50. /**************************************************************************
  51.  *
  52.  * The structure couple is used to store the information required to
  53.  * repeatedly test a pair of polyhedra for collision.  This information
  54.  * includes : a reference to each polyhedron,  a flag indicating if there
  55.  * is a cached prospective proper separating plane, two points for
  56.  * constructing the proper separating plane, and possibly a cached set
  57.  * of points from each polyhedron for speeding up the distance algorithm.
  58.  *
  59.  **************************************************************************/
  60.  
  61. typedef struct couple {
  62.    Polyhedron  polyhdrn1;    /* First polyhedron of collision test. */
  63.    Polyhedron  polyhdrn2;    /* Second polyhedron of collision test. */
  64.    Boolean     plane_exists;    /* prospective separating plane flag */
  65.    double      pln_pnt1[3];    /* 1st point used to form separating plane. */
  66.    double      pln_pnt2[3];    /* 2nd point used to form separating plane. */
  67.    int           vert_indx[4][2]; /* cached points for distance algorithm. */
  68.    int           n;        /* number of cached points, if any. */
  69. } *Couple;
  70.  
  71.  
  72. /*** Arrays for vertex sets of three primitives ***/
  73.  
  74. double box[24];
  75. double cyl[108];
  76. double sphere[1026];
  77.  
  78. /*** RJR 08/20/93 ***********************************************************
  79.  *
  80.  *   Function to create vertex set of a polyhedron used to represent a
  81.  *   box primitive.
  82.  *
  83.  *   On Entry:
  84.  *    box_verts - an empty array of type double of size 24 or more.
  85.  *
  86.  *   On Exit:
  87.  *    box_verts  - vertices of a polyhedron representing a box with
  88.  *             dimensions length = 5.0, width = 5.0, and height = 5.0.
  89.  *
  90.  *   Function Return : none.
  91.  *
  92.  ****************************************************************************/
  93.  
  94. void mak_box(box_verts)
  95. double          box_verts[];
  96. {
  97.    int          i;
  98.    static  double verts[24] =
  99.       {-5.0,  5.0, 5.0,  -5.0,  5.0, -5.0,    5.0,  5.0, -5.0,
  100.       5.0,    5.0, 5.0,  -5.0, -5.0, 5.0,  -5.0, -5.0, -5.0,
  101.       5.0, -5.0, -5.0,  5.0, -5.0, 5.0};
  102.  
  103.    for (i = 0; i < 24; i++)
  104.       box_verts[i] = verts[i];
  105. }
  106.  
  107.  
  108. /*** RJR 08/20/93 ***********************************************************
  109.  *
  110.  *   Function to create vertex set of a polyhedron used to approximate
  111.  *   a cylinder primitive.
  112.  *
  113.  *   On Entry:
  114.  *    cyl_verts - an empty array of type double of size 108 or more.
  115.  *
  116.  *   On Exit:
  117.  *    cyl_verts  - vertices of a polyhedron approximating a cylinder with
  118.  *             a base radius = 5.0, and height = 5.0.
  119.  *   Function Return : none.
  120.  *
  121.  ****************************************************************************/
  122.  
  123. void mak_cyl(cyl_verts)
  124. double          cyl_verts[];
  125. {
  126.    int          i;
  127.    double     *pD_1, *pD_2, rads, stp, radius;
  128.  
  129.    pD_1 = cyl_verts;      pD_2 = cyl_verts + 54;
  130.    stp = 0.34906585;      rads = 0.0;            radius = 5.0;
  131.  
  132.    for (i = 0; i < 18; i++) {
  133.       pD_1[0] = pD_2[0] = radius * cos(rads);       /* X for top and bot. */
  134.       pD_1[1] = 5.0;                   /* Y for top */
  135.       pD_2[1] = 0.0;                   /* Y for bot. */
  136.       pD_1[2] = pD_2[2] = -(radius * sin(rads));   /* Z for top and bot. */
  137.       rads += stp;   pD_1 += 3;      pD_2 += 3;
  138.    }
  139. }
  140.  
  141.  
  142. /*** RJR 08/20/93 ***********************************************************
  143.  *
  144.  *   Function to create vertex set of a polyhedron used to approximate
  145.  *   a sphere primitive.
  146.  *
  147.  *   On Entry:
  148.  *    sph_verts - an empty array of type double of size 1026 or more.
  149.  *
  150.  *   On Exit:
  151.  *    sph_verts  - vertices of a polyhedron approximating a sphere with
  152.  *             a radius = 5.25.
  153.  *   Function Return : none.
  154.  *
  155.  ****************************************************************************/
  156.  
  157. void mak_sph(sph_verts)
  158. double        sph_verts[];
  159. {
  160.    int        i, j;
  161.    double   rads_1, rads_2, stp_1, stp_2, *pD_1, *pD_2, radius;
  162.  
  163.    rads_1 = 1.570796327;   stp_1  = 0.174532935;
  164.    rads_2 = 0.0;       stp_2  = 0.34906585;
  165.    pD_1      = sph_verts;       radius = 5.25;
  166.  
  167.    for (i = 0; i < 19; i++) {
  168.       pD_1[0] = radius * cos(rads_1);      pD_1[1] = radius * sin(rads_1);
  169.       pD_1[2] = 0.0;
  170.       if (EQZ(pD_1[0]))
  171.      pD_1[0] = 0.01;
  172.       rads_2 = 0.0;    stp_2 = 0.34906585;
  173.  
  174.       for (j = 0; j < 18; j++) {
  175.      pD_2 = pD_1 + j * 3;
  176.      pD_2[0] =   pD_1[0] * cos(rads_2) - pD_1[2] * sin(rads_2);  /* X */
  177.      pD_2[1] =   pD_1[1];                         /* Y */
  178.      pD_2[2] = -(pD_1[0] * sin(rads_2) + pD_1[2] * cos(rads_2)); /* Z */
  179.      rads_2 += stp_2;
  180.       }
  181.       pD_1 += 54;   rads_1 -= stp_1;
  182.    }
  183. }
  184.  
  185.  
  186. /*** RJR 05/26/93 ***********************************************************
  187.  *
  188.  *   Function to evaluate the support and contact functions at A for a given
  189.  *   polytope. See equations (6) & (7).
  190.  *
  191.  *   On Entry:
  192.  *    P   - table of 3-element points containing polytope vertices.
  193.  *    r   - number of points in table.
  194.  *    A   - vector at which support and contact functions will be evaluated.
  195.  *    Cp  - empty 3-element array.
  196.  *    P_i - pointer to an int.
  197.  *
  198.  *   On Exit:
  199.  *    Cp  - contact point of P w.r.t. A.
  200.  *    P_i - index into P of contact point.
  201.  *
  202.  *   Function Return :
  203.  *    the result of the evaluation of eq. (6) for P and A.
  204.  *
  205.  ****************************************************************************/
  206.  
  207. double Hp(P, r, A, Cp, P_i)
  208. double         P[][3], A[], Cp[];
  209. int         r, *P_i;
  210. {
  211.    int         i;
  212.    double     max_val, val;
  213.  
  214.    max_val = DOT3(P[0], A);     *P_i = 0;
  215.  
  216.    for (i = 1; i < r; i++) {
  217.       val = DOT3(P[i], A);
  218.       if (val > max_val) {
  219.      *P_i = i;
  220.      max_val = val;
  221.       }
  222.    }
  223.    CPVECTOR3(Cp, P[*P_i]);
  224.  
  225.    return max_val;
  226. }
  227.  
  228.  
  229. /*** RJR 05/26/93 ***********************************************************
  230.  *
  231.  *   Function to evaluate the support and contact functions at A for the
  232.  *   set difference of two polytopes. See equations (8) & (9).
  233.  *
  234.  *   On Entry:
  235.  *    P1   - table of 3-element points containing first polytope's vertices.
  236.  *    m1   - number of points in P1.
  237.  *    P2   - table of 3-element points containing second polytope's vertices.
  238.  *    m2   - number of points in P2.
  239.  *    A    - vector at which to evaluate support and contact functions.
  240.  *    Cs   - an empty array of size 3.
  241.  *    P1_i - a pointer to an int.
  242.  *    P2_i - a pointer to an int.
  243.  *
  244.  *   On Exit:
  245.  *    Cs   - solution to equation 9.
  246.  *    P1_i - index into P1 for solution to equation 9.
  247.  *    P2_i - index into P2 for solution to equation 9.
  248.  *
  249.  *   Function Return :
  250.  *    the result of the evaluation of eq. (8) for P1 and P2 at A.
  251.  *
  252.  ****************************************************************************/
  253.  
  254. double Hs(P1, m1, P2, m2, A, Cs, P1_i, P2_i)
  255. double         P1[][3], P2[][3], A[], Cs[];
  256. int         m1, m2, *P1_i, *P2_i;
  257. {
  258.    double    Cp_1[3], Cp_2[3], neg_A[3], Hp_1, Hp_2;
  259.  
  260.    Hp_1 = Hp(P1, m1, A, Cp_1, P1_i);
  261.  
  262.    CPVECTOR3(neg_A, A);
  263.    VECNEGATE3(neg_A);
  264.    Hp_2 = Hp(P2, m2, neg_A, Cp_2, P2_i);
  265.  
  266.    VECSUB3(Cs ,Cp_1, Cp_2);
  267.  
  268.    return (Hp_1 + Hp_2);
  269. }
  270.  
  271. /*** RJR 05/26/93 ***********************************************************
  272.  *
  273.  *   Alternate function to compute the point in a polytope closest to the
  274.  *   origin in 3-space. The polytope size m is restricted to 1 < m <= 4.
  275.  *   This function is called only when comp_sub_dist fails.
  276.  *
  277.  *   On Entry:
  278.  *    stop_index - number of sets to test.
  279.  *    D_P       - array of determinants for each set.
  280.  *    Di_P       - cofactors for each set.
  281.  *    Is       - indices for each set.
  282.  *    c2       - row size offset.
  283.  *
  284.  *   On Exit:
  285.  *
  286.  *   Function Return :
  287.  *    the index of the set that is numerically closest to eq. (14).
  288.  *
  289.  ****************************************************************************/
  290.  
  291. int sub_dist_back(P, stop_index, D_P, Di_P, Is, c2)
  292. double          P[][3], Di_P[][4], *D_P;
  293. int          stop_index, *Is, c2;
  294. {
  295.    Boolean      first, pass;
  296.    int          i, k, s, is, best_s;
  297.    float      sum, v_aff, best_v_aff;
  298.  
  299.    first = TRUE;  best_s = -1;
  300.    for (s = 0; s < stop_index; s++) {
  301.       pass = TRUE;
  302.       if (D_P[s] > 0.0) {
  303.      for (i = 1; i <= GET(Is,s,0,c2); i++) {
  304.         is = GET(Is,s,i,c2);
  305.         if (Di_P[s][is] <= 0.0)
  306.            pass = FALSE;
  307.      }
  308.       }
  309.       else
  310.      pass = FALSE;
  311.  
  312.       if (pass) {
  313.  
  314.      /*** Compute equation (33) in Gilbert ***/
  315.  
  316.      k = GET(Is,s,1,c2);
  317.      sum = 0;
  318.      for (i = 1; i <= GET(Is, s, 0, c2); i++) {
  319.         is = GET(Is,s,i,c2);
  320.         sum += Di_P[s][is] * DOT3(P[is],P[k]);
  321.      }
  322.      v_aff = sqrt(sum / D_P[s]);
  323.      if (first) {
  324.         best_s = s;
  325.         best_v_aff = v_aff;
  326.         first = FALSE;
  327.      }
  328.      else {
  329.         if (v_aff < best_v_aff) {
  330.            best_s = s;
  331.            best_v_aff = v_aff;
  332.         }
  333.      }
  334.       }
  335.    }
  336.    if (best_s == -1) {
  337.       printf("backup failed\n");
  338.       exit(0);
  339.    }
  340.    return best_s;
  341. }
  342.  
  343. /*** RJR 05/26/93 ***********************************************************
  344.  *
  345.  *   Function to compute the point in a polytope closest to the origin in
  346.  *   3-space. The polytope size m is restricted to 1 < m <= 4.
  347.  *
  348.  *   On Entry:
  349.  *       P  - table of 3-element points containing polytope's vertices.
  350.  *       m  - number of points in P.
  351.  *      jo  - table of indices for storing Dj_P cofactors in Di_P.
  352.  *      Is  - indices into P for all sets of subsets of P.
  353.  *      IsC - indices into P for complement sets of Is.
  354.  *   near_pnt - an empty array of size 3.
  355.  *  near_indx - an empty array of size 4.
  356.  *     lambda - an empty array of size 4.
  357.  *
  358.  *   On Exit:
  359.  *   near_pnt - the point in P closest to the origin.
  360.  *  near_indx - indices for a subset of P which is affinely independent.
  361.  *        See eq. (14).
  362.  *     lambda - the lambda as in eq. (14).
  363.  *
  364.  *   Function Return :
  365.  *    the number of entries in near_indx and lambda.
  366.  *
  367.  ****************************************************************************/
  368.  
  369. int comp_sub_dist(P, m, jo, Is, IsC, near_pnt, near_indx, lambda)
  370. double            P[][3], near_pnt[], lambda[];
  371. int            m, *jo, *Is, *IsC, near_indx[];
  372. {
  373.    Boolean        pass, fail;
  374.    int            i, j, k, isp, is, s, row, col, stop_index, c1, c2;
  375.    double        D_P[15], x[3], Dj_P, Di_P[15][4];
  376.    static int        combinations[5] = {0,0,3,7,15};
  377.  
  378.    stop_index = combinations[m];    /** how many subsets in P **/
  379.    c1 = m;  c2 = m + 1;            /** row offsets for IsC and Is **/
  380.  
  381.    /** Initialize Di_P for singletons **/
  382.  
  383.    Di_P[0][0] = Di_P[1][1] = Di_P[2][2] = Di_P[3][3] = 1.0;
  384.    s = 0;   pass = FALSE;
  385.  
  386.    while ((s < stop_index) && (!pass)) {    /* loop through each subset */
  387.       D_P[s] = 0.0;  fail = FALSE;
  388.       for (i = 1; i <= GET(Is,s,0,c2); i++) {    /** loop through all Is **/
  389.      is = GET(Is,s,i,c2);
  390.      if (Di_P[s][is] > 0.0)            /** Condition 2 Theorem 2 **/
  391.         D_P[s] += Di_P[s][is];        /** sum from eq. (16)      **/
  392.      else
  393.         fail = TRUE;
  394.       }
  395.  
  396.       for (j = 1; j <= GET(IsC,s,0,c1); j++) {    /** loop through all IsC **/
  397.      Dj_P = 0;  k = GET(Is,s,1,c2);
  398.      isp = GET(IsC,s,j,c1);
  399.  
  400.      for (i = 1; i <= GET(Is,s,0,c2); i++) {
  401.         is = GET(Is,s,i,c2);
  402.         VECSUB3(x, P[k], P[isp]);          /** Wk - Wj  eq. (18) **/
  403.         Dj_P += Di_P[s][is] * DOT3(P[is], x); /** sum from eq. (18) **/
  404.      }
  405.      row = GET3(jo,s,isp,0,c1);
  406.      col = GET3(jo,s,isp,1,c1);
  407.      Di_P[row][col] = Dj_P;             /** add new cofactors    **/
  408.  
  409.      if (Dj_P > 0.00001)             /** Condition 3 Theorem 2 **/
  410.         fail = TRUE;
  411.       }
  412.       if ((!fail) && (D_P[s] > 0.0))  /** Conditions 2 && 3 && 1 Theorem 2  **/
  413.      pass = TRUE;
  414.       else
  415.      s++;
  416.    }
  417.    if (!pass) {
  418.       printf("*** using backup procedure in sub_dist\n");
  419.       s = sub_dist_back(P, stop_index, D_P, Di_P, Is, c2);
  420.    }
  421.  
  422.    near_pnt[0] = near_pnt[1] = near_pnt[2] = 0.0;
  423.    j = 0;
  424.    for (i = 1; i <= GET(Is,s,0,c2); i++) {     /** loop through all Is **/
  425.       is = GET(Is,s,i,c2);
  426.       near_indx[j] = is;
  427.       lambda[j] = Di_P[s][is] / D_P[s];               /** eq. (17)  **/
  428.       VECADDS3(near_pnt, lambda[j], P[is], near_pnt);  /** eq. (17)  **/
  429.       j++;
  430.    }
  431.  
  432.    return (i-1);
  433. }
  434.  
  435. /*** RJR 05/26/93 ***********************************************************
  436.  *
  437.  *   Function to compute the point in a polytope closest to the origin in
  438.  *   3-space.  The polytope size m is restricted to 1 < m <= 4.
  439.  *
  440.  *   On Entry:
  441.  *       P  - table of 3-element points containing polytope's vertices.
  442.  *       m  - number of points in P.
  443.  *   near_pnt - an empty array of size 3.
  444.  *  near_indx - an empty array of size 4.
  445.  *     lambda - an empty array of size 4.
  446.  *
  447.  *   On Exit:
  448.  *   near_pnt - the point in P closest to the origin.
  449.  *  near_indx - indices for a subset of P which is affinely independent.
  450.  *        See eq. (14).
  451.  *     lambda - the lambda as in eq. (14).
  452.  *
  453.  *   Function Return :
  454.  *    the number of entries in near_indx and lambda.
  455.  *
  456.  ****************************************************************************/
  457.  
  458. int sub_dist(P, m, near_pnt, near_indx, lambda)
  459. double          P[][3], near_pnt[], lambda[];
  460. int          near_indx[], m;
  461. {
  462.    int          size;
  463.  
  464. /*
  465.  *
  466.  *  Tables to index the Di_P cofactor table in comp_sub_dist.  The s,i
  467.  *  entry indicates where to store the cofactors computed with Is_C.
  468.  *
  469.  */
  470.  
  471.    static int       jo_2[2][2][2]  = { {{0,0}, {2,1}},
  472.                       {{2,0}, {0,0}}};
  473.  
  474.    static int       jo_3[6][3][2]  = { {{0,0}, {3,1}, {4,2}},
  475.                       {{3,0}, {0,0}, {5,2}},
  476.                       {{4,0}, {5,1}, {0,0}},
  477.                       {{0,0}, {0,0}, {6,2}},
  478.                       {{0,0}, {6,1}, {0,0}},
  479.                       {{6,0}, {0,0}, {0,0}}};
  480.  
  481.    static int      jo_4[14][4][2] = { { {0,0}, {4,1}, {5,2}, {6,3}},
  482.                      { {4,0}, {0,0}, {7,2}, {8,3}},
  483.                      { {5,0}, {7,1}, {0,0}, {9,3}},
  484.                      { {6,0}, {8,1}, {9,2}, {0,0}},
  485.                      { {0,0}, {0,0},{10,2},{11,3}},
  486.                      { {0,0},{10,1}, {0,0},{12,3}},
  487.                      { {0,0},{11,1},{12,2}, {0,0}},
  488.                      {{10,0}, {0,0}, {0,0},{13,3}},
  489.                      {{11,0}, {0,0},{13,2}, {0,0}},
  490.                      {{12,0},{13,1}, {0,0}, {0,0}},
  491.                      { {0,0}, {0,0}, {0,0},{14,3}},
  492.                      { {0,0}, {0,0},{14,2}, {0,0}},
  493.                      { {0,0},{14,1}, {0,0}, {0,0}},
  494.                      {{14,0}, {0,0}, {0,0}, {0,0}}};
  495.  
  496.  
  497. /*
  498.  *  These tables represent each Is.  The first column of each row indicates
  499.  *  the size of the set.
  500.  *
  501.  */
  502.    static int      Is_2[3][3] = { {1,0,0}, {1,1,0}, {2,0,1}};
  503.  
  504.    static int      Is_3[7][4] = { {1,0,0,0}, {1,1,0,0}, {1,2,0,0}, {2,0,1,0},
  505.                  {2,0,2,0}, {2,1,2,0}, {3,0,1,2}};
  506.  
  507.    static int     Is_4[15][5] = { {1,0,0,0,0}, {1,1,0,0,0}, {1,2,0,0,0},
  508.                  {1,3,0,0,0}, {2,0,1,0,0}, {2,0,2,0,0},
  509.                  {2,0,3,0,0}, {2,1,2,0,0}, {2,1,3,0,0},
  510.                  {2,2,3,0,0}, {3,0,1,2,0}, {3,0,1,3,0},
  511.                  {3,0,2,3,0}, {3,1,2,3,0}, {4,0,1,2,3}};
  512.  
  513. /*
  514.  *  These tables represent each Is complement. The first column of each row
  515.  *  indicates the size of the set.
  516.  *
  517.  */
  518.    static int     IsC_2[3][2] = { {1,1}, {1,0}, {0,0}};
  519.  
  520.    static int     IsC_3[7][3] = { {2,1,2}, {2,0,2}, {2,0,1}, {1,2,0}, {1,1,0},
  521.                  {1,0,0}, {0,0,0}};
  522.  
  523.    static int    IsC_4[15][4] = { {3,1,2,3}, {3,0,2,3}, {3,0,1,3}, {3,0,1,2},
  524.                  {2,2,3,0}, {2,1,3,0}, {2,1,2,0}, {2,0,3,0},
  525.                  {2,0,2,0}, {2,0,1,0}, {1,3,0,0}, {1,2,0,0},
  526.                  {1,1,0,0}, {1,0,0,0}, {0,0,0,0}};
  527.  
  528.  
  529.    /** Call comp_sub_dist with appropriate tables according to size of P **/
  530.  
  531.    switch (m) {
  532.       case 2:
  533.      size = comp_sub_dist(P, m, jo_2, Is_2, IsC_2, near_pnt,
  534.                                near_indx, lambda);
  535.       break;
  536.       case 3:
  537.      size = comp_sub_dist(P, m, jo_3, Is_3, IsC_3, near_pnt,
  538.                                near_indx, lambda);
  539.       break;
  540.       case 4:
  541.      size = comp_sub_dist(P, m, jo_4, Is_4, IsC_4, near_pnt,
  542.                                 near_indx, lambda);
  543.       break;
  544.    }
  545.  
  546.    return size;
  547. }
  548.  
  549.  
  550. /*** RJR 05/26/93 ***********************************************************
  551.  *
  552.  *   Function to compute the minimum distance between two convex polytopes in
  553.  *   3-space.
  554.  *
  555.  *   On Entry:
  556.  *       P1 - table of 3-element points containing first polytope's vertices.
  557.  *       m1 - number of points in P1.
  558.  *       P2 - table of 3-element points containing second polytope's vertices.
  559.  *       m2 - number of points in P2.
  560.  *       VP - an empty array of size 3.
  561.  *  near_indx - a 4x2 matrix possibly containing indices of initialization
  562.  *        points. The first column are indices into P1, and the second
  563.  *        column are indices into P2.
  564.  *     lambda - an empty array of size 4.
  565.  *       m3 - a pointer to an int, which indicates how many initial points
  566.  *        to extract from near_indx. If 0, near_indx is ignored.
  567.  *
  568.  *   On Exit:
  569.  *     Vp   - vector difference of the two near points in P1 and P2.
  570.  *        The length of this vector is the minimum distance between P1
  571.  *        and P2.
  572.  *  near_indx - updated indices into P1 and P2 which indicate the affinely
  573.  *        independent point sets from each polytope which can be used
  574.  *        to compute along with lambda the near points in P1 and P2
  575.  *        as in eq. (12). These indices can be used to re-initialize
  576.  *        dist3d in the next iteration.
  577.  *     lambda - the lambda as in eqs. (11) & (12).
  578.  *       m3 - the updated number of indices for P1 and P2 in near_indx.
  579.  *
  580.  *   Function Return : none.
  581.  *
  582.  ****************************************************************************/
  583.  
  584. void dist3d(P1, m1, P2, m2, VP, near_indx, lambda, m3)
  585. double            P1[][3], P2[][3], VP[], lambda[];
  586. int            m1, m2, near_indx[][2], *m3;
  587. {
  588.    Boolean        pass;
  589.    int            set_size, I[4], i, j, i_tab[4], j_tab[4], P1_i, P2_i, k;
  590.    double        Hs(), Pk[4][3], Pk_subset[4][3], Vk[3], neg_Vk[3], Cp[3],
  591.             Gp;
  592.  
  593.    if ((*m3) == 0) {         /** if *m3 == 0 use single point initialization **/
  594.       set_size = 1;
  595.       VECSUB3(Pk[0], P1[0], P2[0]);     /** first elementary polytope **/
  596.       i_tab[0] = j_tab[0] = 0;
  597.    }
  598.    else {                 /** else use indices from near_indx **/
  599.       for (k = 0; k < (*m3); k++) {
  600.      i = i_tab[k] = near_indx[k][0];
  601.      j = j_tab[k] = near_indx[k][1];
  602.      VECSUB3(Pk[k], P1[i], P2[j]);     /** first elementary polytope **/
  603.       }
  604.       set_size = *m3;
  605.    }
  606.  
  607.    pass = FALSE;
  608.    while (!pass) {
  609.  
  610.       /** compute Vk **/
  611.  
  612.       if (set_size == 1) {
  613.      CPVECTOR3(Vk, Pk[0]);
  614.      I[0] = 0;
  615.       }
  616.       else
  617.      set_size = sub_dist(Pk, set_size, Vk, I, lambda);
  618.  
  619.       /** eq. (13) **/
  620.  
  621.       CPVECTOR3(neg_Vk, Vk);      VECNEGATE3(neg_Vk);
  622.       Gp = DOT3(Vk, Vk) + Hs(P1, m1, P2, m2, neg_Vk, Cp, &P1_i, &P2_i);
  623.  
  624.       /** keep track of indices for P1 and P2 **/
  625.  
  626.       for (i = 0; i < set_size; i++) {
  627.      j = I[i];
  628.      i_tab[i] = i_tab[j];      /** j is value from member of some Is **/
  629.      j_tab[i] = j_tab[j];      /** j is value from member of some Is **/
  630.       }
  631.  
  632.       if (EQZ(Gp))          /** Do we have a solution **/
  633.      pass = TRUE;
  634.       else {
  635.      for (i = 0; i < set_size; i++) {
  636.         j = I[i];
  637.         CPVECTOR3(Pk_subset[i], Pk[j]);  /** extract affine subset of Pk **/
  638.      }
  639.      for (i = 0; i < set_size; i++)
  640.         CPVECTOR3(Pk[i], Pk_subset[i]);  /** load into Pk+1 **/
  641.  
  642.      CPVECTOR3(Pk[i], Cp);             /** Union of Pk+1 with Cp **/
  643.      i_tab[i] = P1_i;  j_tab[i] = P2_i;
  644.      set_size++;
  645.       }
  646.    }
  647.  
  648.    CPVECTOR3(VP, Vk);              /** load VP **/
  649.    *m3 = set_size;
  650.    for(i = 0; i < set_size; i++) {
  651.       near_indx[i][0] = i_tab[i];      /** set indices of near pnt. in P1 **/
  652.       near_indx[i][1] = j_tab[i];      /** set indices of near pnt. in P2 **/
  653.    }
  654. }
  655.  
  656. /*** RJR 05/26/93 ***********************************************************
  657.  *
  658.  *   Function to compute a proper separating plane between a pair of
  659.  *   polytopes.     The plane will be a support plane for polytope 1.
  660.  *
  661.  *   On Entry:
  662.  *    couple - couple structure for a pair of polytopes.
  663.  *
  664.  *   On Exit:
  665.  *    couple - containing new proper separating plane, if one was
  666.  *         found.
  667.  *
  668.  *   Function Return :
  669.  *    result of whether a separating plane exists, or not.
  670.  *
  671.  ****************************************************************************/
  672.  
  673. Boolean get_new_plane(couple)
  674. Couple          couple;
  675. {
  676.    Polyhedron      polyhedron1, polyhedron2;
  677.    Boolean      plane_exists;
  678.    double      pnts1[MAX_VERTS][3], pnts2[MAX_VERTS][3], dist,
  679.           u[3], v[3], lambda[4], VP[3];
  680.    int          i, k, m1, m2;
  681.  
  682.    plane_exists = FALSE;
  683.  
  684.    polyhedron1 = couple->polyhdrn1;    polyhedron2 = couple->polyhdrn2;
  685.  
  686.    /** Apply M1 to vertices of polytope 1 **/
  687.  
  688.    m1 = polyhedron1->m;
  689.    for (i = 0; i < m1; i++) {
  690.       CPVECTOR3(pnts1[i], polyhedron1->verts[i]);
  691.       VECADD3(pnts1[i], pnts1[i], polyhedron1->trn);
  692.    }
  693.  
  694.    /** Apply M2 to vertices of polytope 1 **/
  695.  
  696.    m2 = polyhedron2->m;
  697.    for (i = 0; i < m2; i++) {
  698.       CPVECTOR3(pnts2[i], polyhedron2->verts[i]);
  699.       VECADD3(pnts2[i], pnts2[i], polyhedron2->trn);
  700.    }
  701.  
  702.    /** solve eq. (1) for two polytopes **/
  703.  
  704.    dist3d(pnts1, m1, pnts2, m2, VP, couple->vert_indx, lambda, &couple->n);
  705.  
  706.    dist = sqrt(DOT3(VP,VP));   /** distance between polytopes **/
  707.  
  708.    if (!EQZ(dist)) {           /** Does a separating plane exist **/
  709.       plane_exists = TRUE;
  710.       u[0] = u[1] = u[2] = v[0] = v[1] = v[2] = 0.0;
  711.       for (i = 0; i < couple->n; i++) {
  712.      k = couple->vert_indx[i][0];
  713.      VECADDS3(u, lambda[i], pnts1[k], u);  /** point in P1 **/
  714.      k = couple->vert_indx[i][1];
  715.      VECADDS3(v, lambda[i], pnts2[k], v);  /** point in P2 **/
  716.       }
  717.  
  718.       /** Store separating plane in P1's local coordinates **/
  719.  
  720.       VECADD3(u, u, polyhedron1->itrn);
  721.       VECADD3(v, v, polyhedron1->itrn);
  722.  
  723.       /** Place separating plane in couple data structure **/
  724.  
  725.       CPVECTOR3(couple->pln_pnt1, u);
  726.       CPVECTOR3(couple->pln_pnt2, v);
  727.    }
  728.    return plane_exists;
  729. }
  730.  
  731. /*** RJR 05/26/93 ***********************************************************
  732.  *
  733.  *   Function to detect if two polyhedra are intersecting.
  734.  *
  735.  *   On Entry:
  736.  *    couple - couple structure for a pair of polytopes.
  737.  *
  738.  *   On Exit:
  739.  *
  740.  *   Function Return :
  741.  *    result of whether polyhedra are intersecting or not.
  742.  *
  743.  ****************************************************************************/
  744.  
  745. Boolean Collision(couple)
  746. Couple          couple;
  747. {
  748.    Polyhedron      polyhedron1, polyhedron2;
  749.    Boolean      collide, loop;
  750.    double      u[3], v[3], norm[3], d;
  751.    int          i, m;
  752.  
  753.    polyhedron1 = couple->polyhdrn1;    polyhedron2 = couple->polyhdrn2;
  754.    collide = FALSE;
  755.  
  756.    if (couple->plane_exists) {
  757.  
  758.       /** Transform proper separating plane to P2 local coordinates.   **/
  759.       /** This avoids the computational cost of applying the           **/
  760.       /** transformation matrix to all the vertices of P2.           **/
  761.  
  762.       CPVECTOR3(u, couple->pln_pnt1);    CPVECTOR3(v, couple->pln_pnt2);
  763.       VECADD3(u, u, polyhedron1->trn);    VECADD3(v, v, polyhedron1->trn);
  764.       VECADD3(u, u, polyhedron2->itrn); VECADD3(v, v, polyhedron2->itrn);
  765.       VECSUB3(norm, v, u);
  766.  
  767.       m = polyhedron2->m;   i = 0; loop = TRUE;
  768.       while ((i < m) && (loop)) {
  769.  
  770.      /** Evaluate plane equation **/
  771.  
  772.      VECSUB3(v, polyhedron2->verts[i], u);
  773.      d = DOT3(v, norm);
  774.  
  775.      if (d <= 0.0) {         /** is P2 in opposite half-space **/
  776.         loop = FALSE;
  777.         if (!get_new_plane(couple)) {
  778.            collide = TRUE;         /** Collision **/
  779.            couple->plane_exists = FALSE;
  780.         }
  781.      }
  782.      i++;
  783.       }
  784.    }
  785.    else
  786.       if (get_new_plane(couple)) {
  787.      couple->plane_exists = TRUE;     /** No Collision **/
  788.       }
  789.       else
  790.      collide = TRUE;         /** Collision **/
  791.  
  792.    return collide;
  793. }
  794.  
  795.  
  796. /*** RJR 05/26/93 ***********************************************************
  797.  *
  798.  *   Function to initialize a polyhedron.
  799.  *
  800.  *   On Entry:
  801.  *   polyhedron - pointer to a polyhedron structure.
  802.  *      verts - verts to load.
  803.  *          m - number of verts.
  804.  *         tx - x translation.
  805.  *         ty - y translation.
  806.  *         tz - z translation.
  807.  *
  808.  *   On Exit:
  809.  *    polyhedron - an initialized polyhedron.
  810.  *
  811.  *   Function Return : none.
  812.  *
  813.  ****************************************************************************/
  814.  
  815. void init_polyhedron(polyhedron, verts, m, tx, ty, tz)
  816. Polyhedron    polyhedron;
  817. double          *verts, tx, ty, tz;
  818. int          m;
  819. {
  820.    int          i;
  821.    double     *p;
  822.  
  823.    polyhedron->trn[0]  =  tx;  polyhedron->trn[1]  =  ty;
  824.    polyhedron->trn[2]  =  tz;
  825.  
  826.    polyhedron->itrn[0] = -tx;  polyhedron->itrn[1] = -ty;
  827.    polyhedron->itrn[2] = -tz;
  828.  
  829.    polyhedron->m = m;
  830.  
  831.    p = verts;
  832.    for (i = 0; i < m; i++) {
  833.       CPVECTOR3(polyhedron->verts[i], p);
  834.       p += 3;
  835.    }
  836. }
  837.  
  838. /*** RJR 05/26/93 ***********************************************************
  839.  *
  840.  *   Function to move a polyhedron.
  841.  *
  842.  *   On Entry:
  843.  *    polyhedron - pointer to a polyhedron.
  844.  *          tx - x translation.
  845.  *          ty - y translation.
  846.  *          tz - z translation.
  847.  *
  848.  *   On Exit:
  849.  *    polyhedron - an updated polyhedron.
  850.  *
  851.  *   Function Return : none.
  852.  *
  853.  ****************************************************************************/
  854.  
  855. void move_polyhedron(polyhedron, tx, ty, tz)
  856. Polyhedron   polyhedron;
  857. double         tx, ty, tz;
  858. {
  859.    polyhedron->trn[0]  += tx;  polyhedron->trn[1]  +=  ty;
  860.    polyhedron->trn[2]  += tz;
  861.  
  862.    polyhedron->itrn[0] -= tx;  polyhedron->itrn[1] -=  ty;
  863.    polyhedron->itrn[2] -= tz;
  864. }
  865.  
  866. /*** RJR 05/26/93 ***********************************************************
  867.  *
  868.  *   This is the Main Program for the Collision Detection example. This test
  869.  *   program creates the vertices of three polyhedra: a sphere, a box, and a
  870.  *   cylinder. The three polyhedra oscillate back and forth along the x-axis.
  871.  *   A collision test is done after each movement on each pair of polyhedra.
  872.  *   This test program was run on an SGI Onyx/4 and an SGI 4D/80.  A total of
  873.  *   30,000 collision detection tests were performed.  There were 3,160
  874.  *   collisions detected. The dist3d function was called in 14% of the
  875.  *   collision tests.  The average number of iterations in dist3d was 1.7.
  876.  *   The above functions are designed to compute accurate solutions when
  877.  *   the polyhedra are simple and convex.  The functions will work on
  878.  *   concave polyhedra, but the solutions are computed using the convex hulls
  879.  *   of the concave polyhedra.    In this case when the algorithm returns a
  880.  *   disjoint result it is exact, but when it returns an intersection result
  881.  *   it is approximate.
  882.  *
  883.  ****************************************************************************/
  884. main()
  885. {
  886.    Polyhedron      Polyhedron1, Polyhedron2, Polyhedron3;
  887.    Couple      Couple1, Couple2, Couple3;
  888.    double      xstp1, xstp2, xstp3;
  889.    int          i, steps;
  890.    long          hits = 0;
  891.  
  892.    /*** Initialize the 3 test polyhedra ***/
  893.  
  894.    mak_box(box);
  895.    mak_cyl(cyl);
  896.    mak_sph(sphere);
  897.  
  898.    Polyhedron1 = (Polyhedron)malloc(sizeof(struct polyhedron));
  899.    init_polyhedron(Polyhedron1, sphere, 342,  0.0, 0.0, 0.0);
  900.  
  901.    Polyhedron2 = (Polyhedron)malloc(sizeof(struct polyhedron));
  902.    init_polyhedron(Polyhedron2, box, 8, 50.0, 0.0, 0.0);
  903.  
  904.    Polyhedron3 = (Polyhedron)malloc(sizeof(struct polyhedron));
  905.    init_polyhedron(Polyhedron3, cyl, 36, -50.0, 0.0, 0.0);
  906.  
  907.    Couple1 = (Couple)malloc(sizeof(struct couple));
  908.    Couple1->polyhdrn1 = Polyhedron1;   Couple1->polyhdrn2 = Polyhedron2;
  909.    Couple1->n = 0;
  910.    Couple1->plane_exists = FALSE;
  911.  
  912.    Couple2 = (Couple)malloc(sizeof(struct couple));
  913.    Couple2->polyhdrn1 = Polyhedron1;   Couple2->polyhdrn2 = Polyhedron3;
  914.    Couple2->n = 0;
  915.    Couple2->plane_exists = FALSE;
  916.  
  917.    Couple3 = (Couple)malloc(sizeof(struct couple));
  918.    Couple3->polyhdrn1 = Polyhedron3;   Couple3->polyhdrn2 = Polyhedron2;
  919.    Couple3->n = 0;
  920.    Couple3->plane_exists = FALSE;
  921.  
  922.    /** Perform Collision Tests **/
  923.  
  924.    xstp1 = 1.0;     xstp2 = 5.0; xstp3 = 10.0;  steps = 10000;
  925.  
  926.    for (i = 0; i < steps; i++) {
  927.       move_polyhedron(Polyhedron1, xstp1, 0.0, 0.0);
  928.       move_polyhedron(Polyhedron2, xstp2, 0.0, 0.0);
  929.       move_polyhedron(Polyhedron3, xstp3, 0.0, 0.0);
  930.  
  931.       if (Collision(Couple1))
  932.      hits++;
  933.       if (Collision(Couple2))
  934.      hits++;
  935.       if (Collision(Couple3))
  936.      hits++;
  937.  
  938.       if (ABS(Polyhedron1->trn[0]) > 100.0)
  939.      xstp1 = -xstp1;
  940.       if (ABS(Polyhedron2->trn[0]) > 100.0)
  941.      xstp2 = -xstp2;
  942.       if (ABS(Polyhedron3->trn[0]) > 100.0)
  943.      xstp3 = -xstp3;
  944.    }
  945.    printf("number of tests = %d\n",(steps * 3));
  946.    printf("number of hits = %ld\n", hits);
  947. }
  948.